#include <encoder/curtis_encoder.h>


namespace encoder {

CurtisEncoder::CurtisEncoder(std::shared_ptr<rclcpp::Node> node, const std::string joint_name)
{
	node_ = node;
	host_name = joint_name;
	joint_name_ = std::string("");
	timestamp_ = node_->now().seconds();
}

CurtisEncoder::~CurtisEncoder()
{
	if (connectionType == ConnectionType::Serial || connectionType == ConnectionType::Tcp) {
		cparam.eth.service->join();
		commPtr.reset();
	}
	
	max_value = 0;
}

void CurtisEncoder::init()
{
	node_->get_parameter_or(host_name + "/encoder/connect_type", connectionType, 1);
    RCLCPP_INFO(node_->get_logger(), "[CurtisEncoder] %s connect type: %d", host_name.c_str(), connectionType);

    node_->get_parameter_or(host_name + "/encoder/looprate", looprate, 50);
    RCLCPP_INFO(node_->get_logger(), "[CurtisEncoder] %s looprate: %d", host_name.c_str(), looprate);

	switch(connectionType) {
	    case ConnectionType::Serial: // 串口
	        cparam.device_name = new std::string();
	        node_->get_parameter_or(
	        	host_name + "/encoder/device_name", *cparam.device_name, std::string("/dev/ttyACM0"));
	        RCLCPP_INFO(node_->get_logger(), 
	        	"[CurtisEncoder] %s serial device name: %s", host_name.c_str(), cparam.device_name->c_str());
	        node_->get_parameter_or(host_name + "/encoder/bandrate", cparam.bandrate, 115200);
	        RCLCPP_INFO(node_->get_logger(), 
	        	"[CurtisEncoder] %s bandrate: %d", host_name.c_str(), cparam.bandrate);
	        commPtr = std::make_shared<Sensor::SerialComm>(node_, *cparam.device_name, cparam.bandrate, looprate);
	        if (commPtr->init() < 0) {
	            RCLCPP_ERROR(node_->get_logger(), "[CurtisEncoder] Serial port init error.");
	            return;
	        }
	        break;
	    case ConnectionType::Tcp: // TCP 服务器
	        max_value = 0xFFFF0000;
	        node_->get_parameter_or(host_name + "/encoder/port", cparam.eth.port, 10002);
	        RCLCPP_INFO(node_->get_logger(), "[CurtisEncoder] %s tcp port: %d", host_name.c_str(), cparam.eth.port);
	        cparam.eth.io = new boost::asio::io_service();
	        commPtr = std::make_shared<Sensor::TcpServerComm>(node_, cparam.eth.port, *cparam.eth.io, looprate);
	        if (commPtr->init() < 0) {
	            RCLCPP_ERROR(node_->get_logger(), "[CurtisEncoder] Encoder port init error.");
	            return;
	        }
	        cparam.eth.service = new boost::thread(boost::bind(&CurtisEncoder::runService, this, cparam.eth.io));
	        break;
	    case ConnectionType::Udp: // UDP 服务器
	        // todo
	        break;
	    case ConnectionType::Can: // CAN 设备 
	    	{
		        cparam.can.device_name = new std::string();
		        std::string default_device_name = "can0";
		        node_->get_parameter_or(host_name + "/encoder/can_device", *cparam.can.device_name, default_device_name);
		        RCLCPP_INFO(node_->get_logger(), 
		        	"[CurtisEncoder] %s can encoder device name is: %s", host_name.c_str(), cparam.can.device_name->c_str());

		        node_->get_parameter_or(host_name + "/encoder/can_nodeId", cparam.can.nodeId, 2);
		        RCLCPP_INFO(node_->get_logger(), 
		        	"[CurtisEncoder] %s can encoder id: %d", host_name.c_str(), cparam.can.nodeId);
		        
		        enc_sub_ = node_->create_subscription<std_msgs::msg::UInt64>(
		            "/" + *cparam.can.device_name + "/device" + std::to_string(cparam.can.nodeId) + "/get_pdo_raw_data_0", 
		            10, 
		            std::bind(&CurtisEncoder::enc_sub_callback, this, std::placeholders::_1));
		        
		        timestamp_sub_ = node_->create_subscription<std_msgs::msg::Float64>(
		            "/" + *cparam.can.device_name + "/device" + std::to_string(cparam.can.nodeId) + "/get_pdo_raw_data_0/timestamp", 
		            10, 
		            std::bind(&CurtisEncoder::timestamp_sub_callback, this, std::placeholders::_1));
	    	}
	        break;
	    default:
	    	RCLCPP_WARN(node_->get_logger(), "[CurtisEncoder] Unknown connection type");
	        break;
	}
}


uint64_t CurtisEncoder::getValue()
{
	uint64_t value = 0;
	switch(connectionType) { 
		case ConnectionType::Serial: 
		case ConnectionType::Tcp: 
			if (commPtr) {
				value = commPtr->read();
				timestamp_ = node_->now().seconds();
				RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
					500, "[CurtisEncoder] read enc: %ld", value);
			}
			break;
		case ConnectionType::Can:
			// value = ((raw_value_ >> 56) & 0xFF) | (((raw_value_ >> 48) & 0xFF) << 8);
			value = raw_value_ & 0xFFFFFFFF;
		default:
			break;
	}

	return value;
}


double CurtisEncoder::getRadValue()
{	
	return 0.0;
}

int64_t CurtisEncoder::getTurns()
{
	int64_t turns = 0;
	switch(connectionType) { 
		case ConnectionType::Serial: 
		case ConnectionType::Tcp: 
			if (commPtr)
				turns = commPtr->get_encoder_turns();
			break;
		case ConnectionType::Can:
			turns = 0;
		default:
			break;
	}

	return turns;
}



void CurtisEncoder::enc_sub_callback(const std_msgs::msg::UInt64::ConstSharedPtr& msg)
{
	raw_value_ = msg->data;
}

void CurtisEncoder::timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg)
{
	timestamp_ = msg->data;
}

void CurtisEncoder::runService(boost::asio::io_service *service)
{
	assert(service != nullptr);
	service->run();
}

}